TaichiSLAM: 一周构建稠密 SLAM 后端的尝试（一）—— Taichi Contributor 徐浩

编者按

这是一篇来自 Taichi Active Contributor 徐浩的技术博客，我们非常荣幸 Taichi 可以对他的研究领域有所助益。徐浩使用 Taichi 进行 mapping 的 SLAM 后端，能够快速遍历特定层次、实现“免费”的并行加速，并可专注于算法实现。同时，徐浩也在 2021 年 9 月参加了太极图形开发者大会（TaichiCon 02 ），并作为嘉宾分享了他用 Taichi 完成的科研工作。

SLAM 全称为 Simultaneous localization and mapping，即实时定位与地图构建，对移动机器人/无人机/自动驾驶/eVTOL 等至关重要。

作者介绍

徐浩 

香港科技大学电子计算机工程系博士在读。本科毕业于中国科学技术大学物理系 主要研究无人机（UAV），无人机集群（Aerial Swarm），无人机集群状态估计，协同定位与建图（CSLAM）。

正文

前言

去年 Taichi^[1]编程语言胡渊鸣《99 行代码的冰雪奇缘》一文在知乎上很算刷了一阵子屏，作为一种计算机图形学的高效开发工具，Taichi 语言已经充分的展现了在这个领域的潜力。

当时看完文章，除了惊叹于作者胡大佬令人惊叹的个人能力以外，也同时产生了一个想法，作为一个用于贴近高性能计算的图形学的领域特定编程语言（DSL），Taichi 是否在机器人学上也有非常好的应用场景。由于当时一篇文章拖了一年，一直没来得及实现这个想法。最近总算有了时间，花了一周时间对 Taichi 的使用算是有了一个初步的了解。在这里写一篇文章总结一下，同时也是传教，从笔者个人的体会出发，涉及到稠密的，“维度灾难”的，高度并行的计算相关应用的朋友都可以去了解一下 Taichi。

为什么使用 Taichi

计算机图形学和机器人学两个学科的关联颇为有趣。在计算机图形学中，开发者们尝试通过最基本的物理法则去构建，模拟整个世界，从物体的运动，液体气体的流动到一个栩栩如生的电影形象，在图形学的大部分时候，这种模拟并不力求物理上的绝对准确：计算机图形学的很多时候是为了视觉效果服务的。当然，既然是做物理模拟，真实性是不可或缺的。为了获得这种真实，计算机图形学的研究者需要使用复杂的数据结构和超大规模的计算进行对物理世界进行模拟仿真。而 Taichi 正是为这种需求设计的。计算图形学完整 pipeline 往往包含从最基本的物理学原理出发（例如 NS 方程），进发到物质的模拟交互仿真（例如物质点法），最后在一个摄像头视角中尽量真实的渲染出来（例如光线追踪）。在物理仿真中，常常涉及到一个问题：稀疏性。

例如在上图的物理仿真中，只要很少一部分空间有粒子。为了节省内存，我们没有必要使用稠密数组去保存空间。而当使用复杂数据结构时，写的不好的数据结构带来的开销（overhead）又过大，问题很容易成为一个 CPU/Mem 的妥协问题。关于这部分，Taichi 的 GAMES201 课程有详细的讨论（编者注：请感兴趣的读者移步哔哩哔哩，观看GAMES201：高级物理引擎实战指南2020，以及今年新推出的更加新手友好的《太极图形课》）。而涉及到稀疏性的数据结构催生了大量的相关算法。Taichi 最大的贡献之一是用户可以很方便的实现这些算法。

与之相反的，移动机器人却需要从物理世界的现实出发，去设计出合格的算法、机器人硬件，与现实世界进行交互。这一路径恰恰和图形学相反。解决的是主要问题包括三个：perception（观测）、planning（规划）、control（控制）。从 perception 出发，任何机器人学的研究要解决的第一个问题是如何观测，测量这个世界。在perception 算法中，我们往往从视觉数据、惯性测量等数据本身出发（例如VIO^[2]），使用这些数据之间的关联去复原这个机器人观察到的世界（SLAM^[3]）。当对世界有了足够的认知以后，我们可以回到最基本的机器人动力学，对我们已知的目标使用动力学约束^[4]来构建可行的路径（planning^[5]），并且使用机器人上的执行器（例如多旋翼的螺旋桨，多足的电机）在最基本的物理定律（例如空气动力学）和数学约束（例如李群^[6]）下执行我们想要的轨迹。更高级的阶段，我们会希望移动机器人有探索世界的能力^[7]。除此之外，对于机器人系统来说，有一个生产环境的“场外观众”，同样非常重要：仿真。实验机器人系统往往昂贵或者极其昂贵，在生产环境中进行大量的测试和 debug 成本极高（例如祝融号火星车，难以进行真正的实验），所以任何成熟的机器人系统都需要仿真环境。

图源自中国航天

一个对系统精确建模的优秀仿真的构建不仅仅限于能帮助 debug，也包括了进行现在较为流行的强化学习^[8]或者神经网络控制器进行训练。DiffTaichi^[9]就是 Taichi 作者使用 Taichi 从物理仿真进行机器人控制器训练的工作。很好的展现了一个好的（甚至可微分的）物理仿真器能带来如何有趣的效果。进一步的来说，对于一些高度非线性的系统，简化后的运动模型也可以嵌入到规划和控制算法中，也就是模型预测控制（model predictive control^[10])。

今天我们的重点将不是使用 Taichi 作为物理仿真，（或许二十年后会有人在无人机上进行实时的 CFD，但是不是现在）而是另一面，使用 Taichi 进行对世界的构建（SLAM)。SLAM 算法涉及到对真实世界的“复现”，考虑到机器人的视野（FoV）往往有限，所以构建出的地图也极为稀疏。

https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping

例如图中，一个很大的场景中，机器人所探索的空间很有限。

作为一个设计用来进行图形学计算的编程语言，我们今天介绍的是如何将他进行“反着用”。利于 Taichi 里面的稀疏数据结构，易于编程和高度并行的特性，快速完成对一些经典算法的复现（可能是简化后的）。

上面我们提到，为了完成图形学的计算，Taichi 有相当强大的并发优化和多样易用的数据结构，同样的，机器人系统作为一个高度实时的系统，对算法性能的优化有着很高的要求。尤其是小型无人机，受限于所谓的 SWaP: Size（大小）、Weight（重量）、Power（功率），小型无人机所搭载的算力极为有限。一些 CV 领域声明实时的算法是在一块甚至数块 TITAN 显卡上完成的，其计算消耗的功率甚至超过了维持小型无人机飞行的功率！另一方面，小型无人机需要处理的任务并不简单，包括实时定位与地图构建（SLAM），planning 和 control。甚至于对于一些集群无人机，分布式的定位^[11]任务分配互相避障也需要大量的计算资源。所以二者结合，或许是一个不错的想法。

本文讨论的是已经有 odometry（里程计），loop detection（回环检测），pose graph optimization（位姿图优化）和 depth cloud（深度点云）或者 depth map（深度图）的情况下，构建稠密地图。实时的 odometry 估计和loop detection、pose graph optimization、深度图生成等不在本文讨论范围。

Octomap^[12]

我们要尝试的第一个算法是简化后 Octomap，可以认为是 occupancy map 的一种。所谓 occupancy map，简单来说是把空间划分为许多格点（voxel），有点像 mincraft。这里我们简化掉了 Octomap 的概率部分，仅仅讨论他的数据结构。

https://www.minecraft.net/zh-hans/article/embrace-past-minecraft-classic

每个 voxel 用占用与否来表示空间中是否有物体，一个简单的稠密 occupancy map 可以直接使用一个三维数组实现，例如

uint8 occupy_map[1000][1000][1000];

当每个格点表示 10 cm 的地图，这就是一个 100 m * 100 m * 100 m 的空间。注意，这一地图已经占用了 1GB 的内存，对于常见的实验室环境计算设备，例如 Jetson TX2，总共只有 8 GB 的共享的显存内存。而考虑到实验室外无人机具有的移动能力，只需要几十秒就可以飞出这 100 m 的地图。

假如我们要设计一个运输无人机或者无人车，如果一座城市的 occupancy map（当然现在没有人会这么干，因为在规划中我们一般只需要局部的地图），例如市区面积不算大的深圳，1,748 平方公里，高度按照 2000 米算，则需要 3179.6 TB 的内存空间。这显然是不现实的。而事实上，这架运输无人机如果各种绕路的穿越整个深圳市区 100 公里长度，每时刻传感器能够覆盖的横截面积 100 m^2，那么一路完整的地图只需要 9 G 就能存下，这不是一个遥不可及的数字。

Octomap 则是提供这样一种思路，把空间逐层使用八叉树分割，也就是 x、y、z 三轴各自二叉树，对于尚未访问的节点，我们不申请内存。如图

Wurm, Kai M., et al. OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems. Proc. of the ICRA 2010 workshop on best practice in 3D perception and modeling for mobile manipulation. Vol. 2. 2010.

那么即使我们申请了一个深圳市大小的地图，我们只使用 1000 m^3 体积时，也只需要 0.95 MB 的内存和 20 层的八叉树。同时，我们只需要从计算根节点是否存在，就可以判断其叶子节点是否被占用，于是获得不同解析度的地图。

八叉树的原理说起来不难，但是实现起来却有不少坑：例如如果我们有 20 层的树，建立了深圳大小的地图，但是却只飞行了 100 m，但是每次我们对空间进行操作，都需要 20 层递归下去，这其中有 10 层是在浪费时间。当然，通过大量的 engineer 操作，你可以进行缓存优化：即第一次查找完以后第二次查找，可以计算出二者的距离，复用第一次的前半部分查找结果等等，不过需要大量的 C++ 代码才能实现。另一个问题是当你希望对某一层进行并行时，同样需要写大量的代码去枚举所有这一层存在的节点。

而使用 Taichi，则实现起来非常容易。简化后的代码如下（可使用的版本见作者 wiki）

#分层构建八叉树
#使用pointer意味着，只有坐标被访问了（写入值）内存才会被申请
for r in range(R):
    B = B.pointer(ti.ijk, (K, K, K))
occupy = ti.field(ti.i32)
B.place(self.occupy)

@ti.kernel
def visit(i, j, k):
    #对occupy 进行写入和激活，我们无需去管树结构，直接把八叉树叶子节点的当做一个三维数组去写入即可
    self.occupy[i, j, k] += 1


@ti.kernel
def retrieve_voxels():
    #遍历某一层的occupy
    tree = self.occupy.parent(level)
    for i, j, k in tree: # For 循环特定层，只有被激活（写入了值）的节点才会被遍历
        if self.occupy[i, j, k] > min_occupy_thres:
            #do something
            pass

Taichi 中 pointer 建立的树结构意味着只有被激活（写入）了值，才会申请内存被后续的操作遍历到。

而后面的访问则可以直接去管八叉树最后的叶子节点，当做一个每一个维度 2^K 大的三维数组处理即可。

例如上面的代码，就完成了对 R 层 k^3 叉树进行构建，写入，对特定层遍历的全过程。当 K=2，就是八叉树，同样的也可以把 K=4，并且减小层数，就成了 64 叉树。用户可以很方便的探索 K 的取值对性能的影响。

注意，上面的代码看起来是单纯的 Python，但其实并不是，所有被 @ti.kernel 修饰的函数（kernel）会被 Taichi 的 LLVM 后端经过即时编译后运行，所以说 Taichi 是一种嵌入在 Python 里的独立编程语言。此外，如果你的 kernel 里面是一个 for-loop，Taichi 会自动帮你把这个 for loop 并行化，并且可以通过改 ti.init(arch=cpu) 或者 ti.init(arch=cuda) 等来切换 CPU 和 CUDA 后端。也就是你可以免费获得并行优化和 CUDA 版本的程序。这也是Taichi 非常激动人心的一点，可以通过类似 Python 的语法直接获得在 CPU/GPU 高度并行化的程序。目前支持的后端包括 CPU/CUDA/Metal/OpenGL。当然还是 CPU 和 CUDA 比较完备。关于这里的细节请阅读 Taichi 作者的知乎文章和课程。

Taichi 有非常好的数据结构优化，例如根据我的实际测量，申请一个 100 km * 100 km * 100 km（矮行尺度差不多能覆盖整个谷神星了）的 octomap 的程序输出

The map voxel is:[16777216x16777216x16777216] all 4.50e+15M grid scale [0.060x0.060x0.060] map scale:[1000000.0mx1000000.0mx1000000.0m] tree depth [24, 24]
...
Time: pcl2npy 2.8ms t_recast 0.7ms ms t_v2p 5.3ms t_pubros 0.0ms t_render 73.9ms
Time: pcl2npy 2.9ms t_recast 0.6ms ms t_v2p 5.3ms t_pubros 0.0ms t_render 80.0ms

和申请一个 100 m * 100 m * 100 m 的网格相比，程序性能几乎没有区别

The map voxel is:[1024x1024x1024] all 1.02e+03M grid scale [0.098x0.098x0.098] map scale:[100.0mx100.0mx100.0m] tree depth [10, 10]
...
Time: pcl2npy 3.7ms t_recast 0.5ms ms t_v2p 6.5ms t_pubros 0.0ms t_render 65.5ms
Time: pcl2npy 2.4ms t_recast 0.4ms ms t_v2p 5.6ms t_pubros 0.0ms t_render 75.4ms

第一个谷神星尺度的 octomap 有 4.50e+15M 的理论 voxel 和 24 层的八叉树，第二个则 1.02e+03M 是理论 voxel 和 10 层的树。使用 Taichi，只需要几行代码就能完成这样的构建。

那么一个简化版本的 octomap 就很简单，只需要把深度图点云根据内外参和机器人的实时位姿（pose）投影到3D 空间中，对应 occupancy+1。当我们需要遍历的时候，直接对特定深度的树进行遍历，并且输出 occupancy 此处超过一个阈值的格点（简易的滤掉噪声）为 occupancy map：

for i, j, k in tree:
    if self.occupy[i, j, k] > min_occupy_thres:
        #output occupy
        pass

此处的遍历（struct-for），Taichi 会自动帮你遍历已经被访问过（+1）了的格点。

效果如图：

较大voxel

较小voxel

有texture的效果。原始数据集来自：https://github.com/ethz-asl/voxblox

大道至简，能用最简单的代码实现不错的效果和高度优化的性能，这个简单的 octomap 证明 Taichi 确实能加速 SLAM 的开发工作。

本文先到此为止，接下来的文章会讲使用 Taichi 建立 TSDF，ESDF 和 submap。

代码在 github.com/xuhao1/Taich ，ESDF，submap 部分尚在开发中。

编后语

太极图形非常欢迎开发者、用户对自己开发或使用 Taichi 的经历、感受进行分享，感兴趣的同学请随时联系我们～

联系邮箱：[e-mail]

参考

1. 1.^https://github.com/taichi-dev/taichi
2. 2.^Qin, Tong, Peiliang Li, and Shaojie Shen. "Vins-mono: A robust and versatile 3. monocular visual-inertial state estimator." IEEE Transactions on Robotics 34.4 (2018): 1004-1020.
3. 3. Kaess, Michael, et al. "iSAM2: Incremental smoothing and mapping using the Bayes tree." The International Journal of Robotics Research 31.2 (2012): 216-235.
4. 4. Mellinger, Daniel, and Vijay Kumar. "Minimum snap trajectory generation and control for quadrotors." 2011 IEEE international conference on robotics and automation. IEEE, 2011.
5. 5. Gao, Fei, et al. "Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments." Journal of Field Robotics 36.4 (2019): 710-733.
6. 6. Yu, Yun, et al. "High-performance full attitude control of a quadrotor on SO (3)." 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015.
7. 7. https://github.com/HKUST-Aerial-Robotics/FUEL
8. 8. Tan, Jie, et al. "Sim-to-real: Learning agile locomotion for quadruped robots." arXiv preprint arXiv:1804.10332 (2018).
9. 9. Hu, Yuanming, et al. "Difftaichi: Differentiable programming for physical simulation." arXiv preprint arXiv:1910.00935 (2019).
10. 10. Agachi, Paul Serban, et al. 2. Model predictive control. De Gruyter, 2016.
11. 11. Xu, Hao, et al. "Decentralized visual-inertial-UWB fusion for relative state estimation of the aerial swarm." 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020.
12. 12. Wurm, Kai M., et al. "OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems." Proc. of the ICRA 2010 workshop on best practices in 3D perception and modeling for mobile manipulation. Vol. 2. 2010.

阅读原文即可前往「太极图形」B 站观看 TaichiCon 02